#include "stable.h"
#include <stdbool.h>

#define SAMPLE_WINDOW 25 // 1秒 = 20个50ms间隔
#define STABILITY_THRESHOLD 1

// 全局变量（也可通过结构体传入函数）
static float yaw_history[SAMPLE_WINDOW]; // 存储最近20次Yaw值
static int history_index = 0;            // 当前历史缓冲区索引
static bool yaw_stable = false;          // Yaw是否已稳定
static float stable_yaw_value = 0.0f;    // 稳定后的Yaw值
float stable_yaw;

/**
 * 检测Yaw稳定性并在稳定时固定值
 * @param Yaw 当前测量的Yaw值
 * @param cnt 时间计数器（每50ms递增1）
 * @return 稳定后的Yaw值（稳定时返回固定值，不稳定时返回当前测量值）
 */
float stabilize_yaw(float Yaw, uint32_t cnt) {
  // 初始化历史缓冲区（首次调用或计数器重置时）
  static uint32_t last_cnt = 0;
  if (cnt < last_cnt || cnt >= last_cnt + SAMPLE_WINDOW) {
    for (int i = 0; i < SAMPLE_WINDOW; i++) {
      yaw_history[i] = Yaw;
    }
    last_cnt = cnt;
    yaw_stable = false;
  }

  // 更新历史缓冲区
  yaw_history[history_index] = Yaw;
  history_index = (history_index + 1) % SAMPLE_WINDOW;
  last_cnt = cnt;

  // 检查是否收集了完整的窗口数据
  if (cnt < SAMPLE_WINDOW) {
    return Yaw; // 数据不足，返回原始值
  }

  // 计算窗口内的最大值和最小值
  float max_yaw = yaw_history[0];
  float min_yaw = yaw_history[0];
  for (int i = 1; i < SAMPLE_WINDOW; i++) {
    if (yaw_history[i] > max_yaw)
      max_yaw = yaw_history[i];
    if (yaw_history[i] < min_yaw)
      min_yaw = yaw_history[i];
  }

  // 计算变化量
  float delta = max_yaw - min_yaw;

  // 判断是否稳定
  if (delta <= STABILITY_THRESHOLD) {
    if (!yaw_stable) {
      // 首次达到稳定条件，记录稳定值
      stable_yaw_value = Yaw;
      yaw_stable = true;
    }
    return stable_yaw_value; // 返回稳定值
  } else {
    yaw_stable = false;
    return 0; // 不稳定，返回0
  }
}

// 获得稳定的yaw值
void Stable_Get(void) {
  if (stable_yaw < 1 && stable_yaw > -1)
    stable_yaw = stabilize_yaw(Yaw, cnt);
  else {
    // yaw = Yaw - stable_yaw;
    // if(yaw>180)yaw =
    // 计算相对于稳定点的角度差
    float raw_diff = Yaw - stable_yaw;

    // 将差值规范化到-180°到180°范围内
    yaw = fmod(raw_diff + 180.0f, 360.0f);
    if (yaw < 0)
      yaw += 360.0f;
    yaw -= 180.0f;
  }
}

// // 获得稳定的yaw值，并将结果规范到0-360°
// void Stable_Get(void) {
//   if (stable_yaw < 1 && stable_yaw > -1) {
//     // 当稳定值在-1到1之间时，持续更新稳定值
//     stable_yaw = stabilize_yaw(Yaw, cnt);
//     // 稳定过程中yaw可暂设为0或当前稳定值（根据需求选择）
//     yaw = 0.0f;  // 或 yaw = stable_yaw;
//   } else {
//     // 计算相对于稳定点的角度差
//     float raw_diff = Yaw - stable_yaw;

//     // 将差值规范化到0°到360°范围内
//     yaw = fmod(raw_diff, 360.0f);  // 取模得到-360到360之间的临时值
//     if (yaw < 0) {
//       yaw += 360.0f;  // 若为负值，加360转为0-360范围
//     }
//   }
// }

// 计算小车需要转动的角度，并限制在-30到30度范围
float calculate_steering_angle(float target_angle, float yaw) {
    // 计算目标角度与当前角度的差值
    float angle_diff = target_angle - yaw;
    
    // 首先将角度差规范到-180到180度（确保计算的是最短路径）
    angle_diff = fmod(angle_diff + 180.0f, 360.0f);
    if (angle_diff < 0) {
        angle_diff += 360.0f;
    }
    angle_diff -= 180.0f;
    
    // 限制角度差在-30到30度之间（小车最大转向角度）
    if (angle_diff > 30.0f) {
        angle_diff = 30.0f;
    } else if (angle_diff < -30.0f) {
        angle_diff = -30.0f;
    }
    
    return angle_diff;
}

// 计算优化后的角度差值（处理180/-180边界跳变）
float calculate_error(float target_angle,float yaw) {
    float error = target_angle - yaw;
    
    // 若差值绝对值超过180°，则取反向短路径（如340°等价于-20°）
    if (error > 180.0f) {
        error -= 360.0f;
    } else if (error < -180.0f) {
        error += 360.0f;
    }
    
    return error;
}
